/***************************************************************************************************************************
 * track_aruco.cpp
 * Author: BENBEN
 * Update Time: 2021.8.4
 *
 * 说明: 单个二维码识别程序，可识别的二维码在Prometheus/Modules/object_detection/config/aruco_images文件夹中
 *      视野里只允许存在一个二维码 且二维码的字典类型要对应
 *      默认二维码的边长为0.2m
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/aruco_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_aruco_det
***************************************************************************************************************************/
#include <ros/ros.h>

#include "state_from_tello.h"
#include "command_to_tello.h"
#include "tello_control_utils.h"
#include "message_utils.h"
#include "control_common.h"
#include"math.h"
#include <uav_drone_msgs/ArucoInfo.h>
#define NODE_NAME "tello_sender"

using namespace std;
 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
uav_drone_msgs::ControlCommand Command_Now;                      //无人机当前执行命令
uav_drone_msgs::ControlCommand Command_Last;                     //无人机上一条执行命令

uav_drone_msgs::ArucoInfo Aruco_Now;                      //无人机当前执行命令

Eigen::Vector3d state_sp(0,0,0);
Eigen::Vector3d state_sp_extra(0,0,0);
double yaw_sp;
double yaw_rate_sp;

uav_drone_msgs::Message message;
uav_drone_msgs::LogMessageControl LogMessage;

//订阅与发布话题

ros::Publisher message_pub;
ros::Publisher log_message_pub;
ros::Subscriber aruco_pose_sub;
ros::Subscriber Command_sub;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
string mav_name;
float yaw_speed;
float flight_x_speed;
float flight_y_speed;
float flight_z_speed;
float track_x_max;
float track_x_min;
float track_y_max;
float track_y_min;
float track_z_max;
float track_z_min;


//状态信息
bool takeoff_flag= false;;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param();

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void Command_cb(const uav_drone_msgs::ControlCommand::ConstPtr& msg)
{
    // CommandID必须递增才会被记录(目前不知道会不会增加，先屏蔽该项目，直接写入消息)
    Command_Now = *msg;
}
void timerCallback(const ros::TimerEvent& e)
{
    pub_message(message_pub, uav_drone_msgs::Message::NORMAL, NODE_NAME, "Program is running.");
}

void Aruco_cb(const uav_drone_msgs::ArucoInfo::ConstPtr& msg)
{
    Aruco_Now = *msg;
}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "tello_sender");
    ros::NodeHandle nh("~");
    nh.param<string>("mav_name", mav_name, "tello");
    nh.param<float>("yaw_speed", yaw_speed, 0.2);
    nh.param<float>("flight_x_speed", flight_x_speed, 10.0);
    nh.param<float>("flight_y_speed", flight_y_speed, 10.0);
    nh.param<float>("flight_z_speed", flight_z_speed, 10.0);
    nh.param<float>("track_x_max", track_x_max, 10.0);
    nh.param<float>("track_x_min", track_x_min, -10.0);
    nh.param<float>("track_y_max", track_y_max, 10.0);
    nh.param<float>("track_y_min", track_y_min, -10.0);
    nh.param<float>("track_z_max", track_z_max, 50.0);
    nh.param<float>("track_z_min", track_z_min, 30.0);
    // 用于TELLO控制的类，通过tello_node(tellopy)发送控制指令至飞控【本程序->tellopy->tello】
    command_to_tello _command_to_tello;
    //【订阅】指令
    // 本话题为任务模块生成的控制指令
    Command_sub = nh.subscribe<uav_drone_msgs::ControlCommand>("/sylu_drone/control_command", 10, Command_cb);
    aruco_pose_sub = nh.subscribe<uav_drone_msgs::ArucoInfo>("/tello/object_detection/aruco_det",10,Aruco_cb);
    // 【发布】用于地面站显示的提示消息
    message_pub = nh.advertise<uav_drone_msgs::Message>("/"+mav_name+"/message/main", 10);

    // 10秒定时打印，以确保程序在正确运行
    ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);
    // 建议控制频率 ： 1Hz甚至更低,     TELLO需要执行完一条指令后在执行下一条，所以需要一些空余时间
    ros::Rate rate(1);

    printf_param();
    
    // 初始化命令-
    // 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
    Command_Now.Mode                                                                    = uav_drone_msgs::ControlCommand::Idle;
    Command_Now.Command_ID                                                   = 0;
    Command_Now.Reference_State.Move_mode                   = uav_drone_msgs::PositionReference::XYZ_POS;
    Command_Now.Reference_State.Move_frame                   = uav_drone_msgs::PositionReference::ENU_FRAME;
    Command_Now.Reference_State.position_ref[0]              = 0;
    Command_Now.Reference_State.position_ref[1]              = 0;
    Command_Now.Reference_State.position_ref[2]              = 0;
    Command_Now.Reference_State.yaw_ref                            = 0;


    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
    float last_time = tello_control_utils::get_time_in_sec(begin_time);

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {

        // 执行回调函数
        ros::spinOnce();
        switch (Command_Now.Mode)
        {

        // 【Takeoff】 从摆放初始位置原地起飞至指定高度（高度不可更改），偏航角只能是0
        case uav_drone_msgs::ControlCommand::Takeoff:
            takeoff_flag = true;
            _command_to_tello.takeoff(); 
        break;

        // 【Hold】 悬停。当前位置悬停
        //       TELLO的CMD_VEL是控制油门+欧拉角比例，此时欧拉角比例为0，油门不变
        //      但当前没有状态量回传
        case uav_drone_msgs::ControlCommand::Hold:

            if (Command_Last.Mode != uav_drone_msgs::ControlCommand::Hold)
            {
                Command_Now.Reference_State.Move_mode       = uav_drone_msgs::PositionReference::XYZ_POS;
                Command_Now.Reference_State.Move_frame      = uav_drone_msgs::PositionReference::ENU_FRAME;
                //根据TELLO_NODE和TELLOPY可以得知
                //油门【position_ref[2]】为正数---> 上升，负数  ---->下降，偏航为左或者右
                Command_Now.Reference_State.position_ref[0] = 0;
                Command_Now.Reference_State.position_ref[1] = 0;
                Command_Now.Reference_State.position_ref[2] = 0;
                Command_Now.Reference_State.yaw_ref               = 0; 

                state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
                yaw_sp = Command_Now.Reference_State.yaw_ref ; //-1 至 1
            }
            _command_to_tello.send_pos_setpoint(state_sp, yaw_sp);

            break;

        // 【Land】 降落，直接发送land话题命令，使用TELLOPY的land函数
        case uav_drone_msgs::ControlCommand::Land:
            takeoff_flag = false;
            _command_to_tello.land();
        break;

        case uav_drone_msgs::ControlCommand::Move:
            state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
            yaw_sp = Command_Now.Reference_State.yaw_ref;
            _command_to_tello.send_pos_setpoint(state_sp, yaw_sp);
        break;


        // 【Disarm】 上锁，TELLO不需要上锁
        case uav_drone_msgs::ControlCommand::Disarm:
            ROS_INFO("NOW，Disarm is not support");
        break;

        // 【User_Mode1】 TELLO跟踪二维码
        case uav_drone_msgs::ControlCommand::User_Mode1:
        //有一个方向是不动的，定义横滚方向不动
        Command_Now.Reference_State.position_ref[1] = 0;
            if(Aruco_Now.des_detected == true)
            {
                if(Aruco_Now.position[0] <= track_x_min)    //小于设定的最小值时，旋转
                {
                    yaw_sp = 0-yaw_speed;
                }
                else if (Aruco_Now.position[0] <= track_x_max)
                {
                    yaw_sp = 0;
                }
                else
                {
                    yaw_sp = yaw_speed;
                }

                if(Aruco_Now.position[1] >= track_y_max)    //小于设定的最小值时，上下，因为Y正方向向下，所以大于
                {
                    //因为Y正方向向下，所以大于某值时，向下
                    Command_Now.Reference_State.position_ref[2] = 0-flight_z_speed;
                }
                else if (Aruco_Now.position[1] >=  track_y_min)
                {
                    Command_Now.Reference_State.position_ref[2] = 0;
                }
                else
                {
                    Command_Now.Reference_State.position_ref[2] = flight_z_speed;
                }

                if(Aruco_Now.position[2] <= track_z_min)    //小于设定的最小值时，前后
                {
                    Command_Now.Reference_State.position_ref[0] = 0-flight_x_speed;
                }
                else if (Aruco_Now.position[2] <= track_z_max)
                {
                    Command_Now.Reference_State.position_ref[0] = 0;
                }
                else
                {
                    Command_Now.Reference_State.position_ref[0] = flight_x_speed;
                }  
            }
            else//表示没有检测到所需的Aruco ID，此时保持原位悬停
            {
                Command_Now.Reference_State.position_ref[0] = 0;
                Command_Now.Reference_State.position_ref[1] = 0;
                Command_Now.Reference_State.position_ref[2] = 0;
                Command_Now.Reference_State.yaw_ref               = 0; 
                state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
                yaw_sp = Command_Now.Reference_State.yaw_ref ; //-1 至 1
                ROS_INFO("Don't detection Aruco id = 1, You can try other Aruco image ");
            }
            //发送控制指令
            state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
            _command_to_tello.send_pos_setpoint(state_sp, yaw_sp);
        break;

        // 【User_Mode2】 暂空。可进行自定义
        case uav_drone_msgs::ControlCommand::User_Mode2:
            ROS_INFO("NOW，User_Mode2 is not support，you can do it");
        break;
        }

        Command_Last = Command_Now;
        rate.sleep();
    }

    return 0;

}


void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>> tello_sender Parameter <<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "yaw_speed   : "<< yaw_speed<<" [gain] "<<endl;
    cout << "x_speed   : "<< flight_x_speed<<" [cm/s] "<<endl;
    cout << "y_speed   : "<< flight_y_speed<<" [cm/s] "<<endl;
    cout << "z_speed   : "<< flight_z_speed<<" [cm/s] "<<endl;
    cout << "track_x_max   : "<< track_x_max<<" [cm] "<<endl;
    cout << "track_x_min   : "<< track_x_min<<" [cm] "<<endl;
    cout << "track_y_max   : "<< track_y_max<<" [cm] "<<endl;
    cout << "track_y_min   : "<< track_y_min<<" [cm] "<<endl;
    cout << "track_z_max   : "<< track_z_max<<" [cm] "<<endl;
    cout << "track_z_min   : "<< track_z_min<<" [cm] "<<endl;
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>  <<<<<<<<<<<<<<<<<<< <<<<<<<<<<<<<<<<<<<<<<" <<endl;
}

